package com.orange.rd.sophia.android.sensorscharts.tools;

import java.util.ArrayList;

import com.orange.rd.sophia.android.sensorscharts.SensorsChartsApplication;
import com.orange.rd.sophia.android.sensorscharts.conf.Properties;
import com.orange.rd.sophia.android.sensorscharts.db.SensorData;


import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.util.FloatMath;


public class SensorsDataManager implements SensorEventListener {

	private boolean isRegistered;
	private Sensor sensor;
	private SensorData lastVal = null;
	private long lastTime;
	private double computedVal;
	private int sensorType;
	private int filterType;
	private boolean withAdaptativeFilter;
	private static float[] orientation = null;
	private static float[] rotation = null;
	private SensorManager sm;
	private int nbCalibrations = 0;
	private float calibrationX = 0;
	private float calibrationY = 0;
	private float calibrationZ = 0;
	public final static int NB_CALIBRATIONS = 10;
	public final static int NB_SECOND_COLLECT = 1; // Collect the data for 1 second 
	private boolean withCalibration;
    // rotation matrix from gyro data
    private static float[] gyroMatrix = new float[9];
	private static boolean initState = true;
	private static float timestamp;
    // final orientation angles from sensor fusion
    private static float[] fusedOrientation = new float[3];
	public static final float FILTER_COEFFICIENT = 0.98f;

	private ArrayList<SensorData> sensorDatas;

	private ArrayList<SensorObserverInterface> sensorObs;

	private int sensorDelay;
	// Arbitrary values: filterAlpha ~ 5/(5+1)=0.83333
	public double dt;
	private float RC = 5;
	private double filterAlpha = RC / (RC + 1);
	private double alpha = filterAlpha;
	private float kAccelerometerMinStep = 0.033f;
	private float kAccelerometerNoiseAttenuation = 3.0f;
	public final static double MULT_NANO = 0.000000001D;

	private float r = 4; // Noise factor of the sensor (Hz)
	private float p = 30; // Initial estimated error (will be adjusted during
							// process)
	private float q = 10; // Process noise (Hz)
	private float k = p / (p + r); // Kalman gain

	private boolean isFirst = true;
	private boolean isHeader = true;
	private static boolean success = false;

	private double nbSeconds = 0;
	private int nbTry = 0;

	public final static int TYPE_FILTER_NONE = 0;
	public final static int TYPE_FILTER_LOW_PASS = 1;
	public final static int TYPE_FILTER_HIGH_PASS = 2;
	public final static int TYPE_FILTER_KALMAN = 3;
	public final static int TYPE_FILTER_COMPLEMANTARY = 4;

	public final static int SENSOR_TYPE_ACCELEROMETER_WITHOUT_GRAVITY = -9;
	public final static int SENSOR_TYPE_ACCELEROMETER = Sensor.TYPE_ACCELEROMETER;
	public final static int SENSOR_TYPE_GYROSCOPE = Sensor.TYPE_GYROSCOPE;
	public final static int SENSOR_TYPE_MAGNETIC_FIELD = Sensor.TYPE_MAGNETIC_FIELD;
	public final static int SENSOR_TYPE_GRAVITY = Sensor.TYPE_GRAVITY;
	public final static int SENSOR_TYPE_ORIENTATION = Sensor.TYPE_ORIENTATION;
	public final static int SENSOR_TYPE_LINERA_ACCEL = Sensor.TYPE_LINEAR_ACCELERATION;
	public final static int SENSOR_TYPE_ROTATION_VECTOR = Sensor.TYPE_ROTATION_VECTOR;

	public final static int SENSOR_TYPE_OTHER = -1;

	// magnetic field sensor data manager
	public static SensorsDataManager magneticFieldData = new SensorsDataManager(
			SensorsDataManager.SENSOR_TYPE_MAGNETIC_FIELD,
			Properties.filterType, Properties.withAdaptativeFilter, false);
	// accelerometer sensor data manager
	public static SensorsDataManager accelerometerData = new SensorsDataManager(
			SensorsDataManager.SENSOR_TYPE_ACCELEROMETER,
			Properties.filterType, Properties.withAdaptativeFilter, false);
	// gravity sensor data manager
	public static SensorsDataManager gravityData = new SensorsDataManager(
			SensorsDataManager.SENSOR_TYPE_GRAVITY,
			Properties.filterType, Properties.withAdaptativeFilter, false);
	// orientation sensor data manager
	public static SensorsDataManager orientationData = new SensorsDataManager(
			SensorsDataManager.SENSOR_TYPE_ORIENTATION,
			Properties.filterType, Properties.withAdaptativeFilter, false);
	// linear acceleration sensor data manager
	public static SensorsDataManager linearAccelData = new SensorsDataManager(
			SensorsDataManager.SENSOR_TYPE_LINERA_ACCEL,
			Properties.filterType, Properties.withAdaptativeFilter, 
				Properties.calibrateLinearAccel);
	// rotation sensor data manager
	public static SensorsDataManager rotationData = new SensorsDataManager(
			SensorsDataManager.SENSOR_TYPE_ROTATION_VECTOR,
			Properties.filterType, Properties.withAdaptativeFilter, false);

	// selected sensor data manager by the user: default is linear acceleration
	public static SensorsDataManager gyroscopeData = new SensorsDataManager(
	SensorsDataManager.SENSOR_TYPE_GYROSCOPE,
	TYPE_FILTER_COMPLEMANTARY, false, false);
	
	public static SensorsDataManager selectedData = linearAccelData;

	public SensorsDataManager(int sensorType, int filterType,
			boolean withAdaptativeFilter, boolean withCalibration) {
		sm = (SensorManager) SensorsChartsApplication.getAppContext()
				.getSystemService(Context.SENSOR_SERVICE);
		this.sensorType = sensorType;
		this.filterType = filterType;
		this.withAdaptativeFilter = withAdaptativeFilter;
		this.withCalibration = withCalibration;
		FileUtilities.deleteFile(Properties.SD_DIRECTORY + getSensorTypeName() + ".txt");
		sensorDatas = new ArrayList<SensorData>();
		sensorObs = new ArrayList<SensorObserverInterface>();
		isRegistered = false;
		sensor = sm.getDefaultSensor(sensorType);
		this.sensorDelay = Properties.sensorDelay;
		nbSeconds=0;
	}

	public void register() {
		if (isRegistered) {
			sm.unregisterListener(this);
			isRegistered = false;
		}
		isFirst = true;
		if (sensor!=null) {
			sm.registerListener(this, sensor, sensorDelay);
			isRegistered = true;
		}
	}
	
	public void unregister() {
		sensorObs.clear();
		if (isRegistered) { 
			sm.unregisterListener(this, sensor);
			isRegistered = false;
		}
	}
	
	@Override
	public void finalize() {
		unregister();
		try {
			super.finalize();
		} catch (Exception e) {
		} catch (Throwable t) {
		}
	}
	
	public boolean addObserver(SensorObserverInterface obs) {
		boolean res = true;
		if (!sensorObs.contains(obs))
			res = sensorObs.add(obs);
		register();
		return res;
	}

	public boolean removeObserver(SensorObserverInterface obs) {
		boolean res = false;
		if (sensorObs.size()>0)
			if (sensorObs.contains(obs))
				res = sensorObs.remove(obs);
		// No more observer: unregister
		if (sensorObs.size()==0) {
			unregister();
		}
		return res;
	}

	public float[] addSensorData(SensorData sensorData) {
		// Computed linear acceleration
		boolean ok = false;

		if (sensorType==SENSOR_TYPE_MAGNETIC_FIELD) {// Process only once
			if (accelerometerData.getSensorDatas().size() > 0) {
				SensorData accel =accelerometerData.getSensorDatas().get(
						accelerometerData.getSensorDatas().size() - 1);
				float[] magnetoValues = sensorData.val;
				float[] acceleration = accel.val;
				float R[] = new float[9];
				float I[] = new float[9];
				success = SensorManager.getRotationMatrix(R, I,
						acceleration, magnetoValues);
				if (success) {
					orientation = SensorManager.getOrientation(R, new float[3]);
				}
			}
		}
		
		if (orientation!=null) {
			if (sensorType == SENSOR_TYPE_ACCELEROMETER_WITHOUT_GRAVITY) {
				float gx = (SensorManager.GRAVITY_EARTH * FloatMath.sin(orientation[2]));
				float gy = (SensorManager.GRAVITY_EARTH * FloatMath.sin(orientation[1]));
				float gz = (SensorManager.GRAVITY_EARTH * (FloatMath.cos(orientation[1]) 
						* FloatMath.cos(orientation[2])));
	
				sensorData.val[0] -= gx;
				sensorData.val[1] -= gy;
				sensorData.val[2] -= gz;
	
				sensorData.filteredVal[0] -= gx;
				sensorData.filteredVal[1] -= gy;
				sensorData.filteredVal[2] -= gz;
			}
			sensorData.setOrientation(orientation);
		}
		
		if (sensorType==SENSOR_TYPE_ROTATION_VECTOR) {
			rotation=sensorData.val;
			float vec[] = sensorData.val;
			float quat[] = new float[4];
			SensorManager.getQuaternionFromVector(quat, vec);
			float [] RotMat = new float[9];
			SensorManager.getRotationMatrixFromVector(RotMat, quat);
			rotation = RotMat;
		}
		
		if (rotation!=null)
			sensorData.rotation = rotation;
		
		sensorDatas.add(sensorData);

		if (isFirst) {
			return addFirst(sensorData);
		}

		// Collect CSV data
		if (Properties.saveDataInLocalFile && isHeader) {
			String header = "Filter,Alpha,Time,X,Y,Z,X',Y',Z'"+"\n";
			FileUtilities.saveDataToFile(Properties.SD_DIRECTORY + getSensorTypeName() + ".txt",header);
			isHeader = false;
		}
		
		if (withCalibration) {
			sensorData.val[0] -= calibrationX;
			sensorData.val[1] -= calibrationY;
			sensorData.val[2] -= calibrationZ;
		}

		dt = (sensorData.time - lastTime) * MULT_NANO;
		nbSeconds+=dt;
		lastTime = sensorData.time;

		if (sensorType==SENSOR_TYPE_GYROSCOPE) {
			if (orientation!=null) {
				// Complementary filter
				sensorData.filteredVal = getFilteredVal(filterType, lastVal.filteredVal, sensorData.val, orientation, dt);
		    	System.arraycopy(sensorData.val, 0, fusedOrientation, 0, 3);     
		        // initialisation of the gyroscope based rotation matrix
		        if(initState) {
		            /*
		             * Code provided by :
		             * http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/	
		             */
		            float[] initMatrix = new float[9];
		            // initialise gyroMatrix with identity matrix
		            gyroMatrix[0] = 1.0f; 
		            gyroMatrix[4] = 1.0f; 
		            gyroMatrix[8] = 1.0f;
		            initMatrix = getRotationMatrixFromOrientation(orientation);
		            gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
		            initState = false;
		        }       
		        if(dt != 0) {         
			        // copy the new gyro values into the gyro array     
			        // convert the raw gyro data into a rotation vector     
			        float[] deltaVector = new float[4];     
		        	getRotationVectorFromGyro(fusedOrientation, deltaVector, dt / 2.0f);     
			        // convert rotation vector into rotation matrix     
			        float[] deltaMatrix = new float[9];     
			        SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);       
			        // apply the new rotation interval on the gyroscope based rotation matrix     
			        gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);       
			        // get the gyroscope based orientation from the rotation matrix     
			        SensorManager.getOrientation(gyroMatrix, fusedOrientation); 				        
			        // overwrite gyro matrix and orientation with fused orientation         
			        // to comensate gyro drift         
			        gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);         
		        }
		        // overide yaw
				sensorData.setFusedOrientation(fusedOrientation);
			}
		} else {
			sensorData.filteredVal = getFilteredVal(filterType, lastVal.val, sensorData.val, lastVal.filteredVal, dt);
		}
		if (Properties.saveDataInLocalFile) {
			String data = getFilterTypeName() + "," + alpha
					+ "," + sensorData.time + "," + sensorData.val[0] + ","
					+ sensorData.val[1] + "," + sensorData.val[2] + ","
					+ sensorData.filteredVal[0] + "," + sensorData.filteredVal[1] + "," + sensorData.filteredVal[2];
	
			FileUtilities.saveDataToFile(Properties.SD_DIRECTORY + getSensorTypeName() + ".txt",	data);
		}

		lastVal = sensorData;

		if (nbSeconds>=NB_SECOND_COLLECT) {
		for (SensorObserverInterface obs : sensorObs) {
				obs.updateSensorData(sensorDatas, sensorType, filterType,
						withAdaptativeFilter, dt, sensorData);
			}
			nbSeconds = 0;
			sensorDatas = new  ArrayList<SensorData>();
		}

		return sensorData.filteredVal;
	}
	
	private float[] getFilteredVal(int filterType, float[] lastVal, float[] current, float[] filtered, double dt) {
		float valFiltered[] = { 0, 0, 0 };
		filterAlpha = RC / (dt + RC);
		alpha = filterAlpha;

		if (withAdaptativeFilter) {
			double d = clamp(
					Math.abs(norm(lastVal[0], lastVal[1], lastVal[2])
							- norm(current[0], current[1], current[2]))
							/ kAccelerometerMinStep - 1.0f, 0.0f, 1.0f);
			alpha = (float) (d * filterAlpha / kAccelerometerNoiseAttenuation + (1.0f - d)
					* alpha);
		}

		switch (filterType) {
		case TYPE_FILTER_NONE: // No filter
			valFiltered[0] = current[0];
			valFiltered[1] = current[1];
			valFiltered[2] = current[2];
			break;
		case TYPE_FILTER_LOW_PASS: // Low-pass filter
			if (lastVal==null)
				return getFilteredVal(TYPE_FILTER_NONE, lastVal, current, filtered, dt);
        	alpha = FILTER_COEFFICIENT/(FILTER_COEFFICIENT+dt);
			valFiltered[0] = (float) (current[0] * alpha + lastVal[0] * (1.0 - alpha));
			valFiltered[1] = (float) (current[1] * alpha + lastVal[1] * (1.0 - alpha));
			valFiltered[2] = (float) (current[2] * alpha + lastVal[2] * (1.0 - alpha));
			break;
		case TYPE_FILTER_HIGH_PASS: // High-pass filter
			if (lastVal==null)
				return getFilteredVal(TYPE_FILTER_NONE, lastVal, current, filtered, dt);
			valFiltered[0] = (float) (alpha * (filtered[0] + current[0] - lastVal[0]));
			valFiltered[1] = (float) (alpha * (filtered[1] + current[1] - lastVal[1]));
			valFiltered[2] = (float) (alpha * (filtered[2] + current[2] - lastVal[2]));
			break;
		case TYPE_FILTER_KALMAN: // Simple one dimensional Kalman filter
			if (lastVal==null)
				return getFilteredVal(TYPE_FILTER_NONE, lastVal, current, filtered, dt);
			k = p / (p + r);
			alpha = k;
			valFiltered[0] = (float) (lastVal[0] + alpha	* (current[0] - lastVal[0]));
			valFiltered[1] = (float) (lastVal[1] + alpha	* (current[1] - lastVal[1]));
			valFiltered[2] = (float) (lastVal[2] + alpha	* (current[2] - lastVal[2]));
			p = (1 - k) * p;
			break;
		case TYPE_FILTER_COMPLEMANTARY: // Used only for fused orientation
        	if (filtered==null || orientation==null) {
        		return  getFilteredVal(TYPE_FILTER_LOW_PASS, lastVal, current, current, dt);
        	}
        	alpha = FILTER_COEFFICIENT/(FILTER_COEFFICIENT+dt);
        	 // Calculate the angular speed of the sample     
			valFiltered[0] = (float) ((alpha * current[0]) + (1 - alpha) * orientation[0]);
			valFiltered[1] = (float) ((alpha * current[1]) + (1 - alpha) * orientation[1]);
			valFiltered[2] = (float) ((alpha * current[2]) + (1 - alpha) * orientation[2]);
			break;
		}
		return valFiltered; 
	}

	private float[] addFirst(SensorData sensor) {
		lastVal = sensor;
		setLastTime(sensor.time);
		computedVal = 0;
		p = p + q;
		if (withCalibration) {
			if (nbCalibrations >= NB_CALIBRATIONS) {
				doCalibration();
				setFirst(false);
			}
			nbCalibrations++;
		} else {
			setFirst(false);
		}
		return sensor.val;
	}

	private void doCalibration() {
		for (SensorData s : sensorDatas) {
			calibrationX += 0 - s.val[0];
			calibrationY += 0 - s.val[1];
			calibrationZ += 0 - s.val[2];
		}
		int nbData = sensorDatas.size();
		calibrationX /= nbData;
		calibrationY /= nbData;
		calibrationZ /= nbData;
//		Log.i("CALIBRATION", "X=" + calibrationX + " Y=" + calibrationY + " Z="
//				+ calibrationZ);
	}

	public SensorData getLastData() {
		int size = sensorDatas.size();
		if (size == 0) {
			return null;
		}
		return sensorDatas.get(size);
	}

	public static double norm(double x, double y, double z) {
		return Math.sqrt(x * x + y * y + z * z);
	}

	public static double clamp(double v, double min, double max) {
		if (v > max)
			return max;
		else if (v < min)
			return min;
		else
			return v;
	}

	public long getLastTime() {
		return lastTime;
	}

	private void setLastTime(long lastTime) {
		this.lastTime = lastTime;
	}

	public boolean isFirst() {
		return isFirst;
	}

	public void setFirst(boolean isFirst) {
		this.isFirst = isFirst;
	}

	public ArrayList<SensorData> getSensorDatas() {
		return sensorDatas;
	}

	public int getSensorType() {
		return sensorType;
	}

	public void setSensorType(int type) {
		sensorType = type;
	}

	public int getFilterType() {
		return filterType;
	}

	public void setFilterType(int type) {
		filterType = type;
	}

	public boolean isWithAdaptativeFilter() {
		return withAdaptativeFilter;
	}

	public void isWithAdaptativeFilter(boolean is) {
		withAdaptativeFilter = is;
	}

	public String getSensorTypeName() {
		return getSensorTypeName(sensorType);
	}

	public static String getSensorTypeName(int type) {
		String name = "UNKNOWN";
		switch (type) {
		case SENSOR_TYPE_ACCELEROMETER:
			name = "Accelerometer";
			break;
		case SENSOR_TYPE_ACCELEROMETER_WITHOUT_GRAVITY:
			name = "Accelerometer without gravity";
			break;
		case SENSOR_TYPE_GYROSCOPE:
			name = "Gyroscope";
			break;
		case SENSOR_TYPE_MAGNETIC_FIELD:
			name = "Magnetic field";
			break;
		case SENSOR_TYPE_GRAVITY:
			name = "Gravity";
			break;
		case SENSOR_TYPE_ORIENTATION:
			name = "Orientation";
			break;
		case SENSOR_TYPE_LINERA_ACCEL:
			name = "Linear acceleration";
			break;
		case SENSOR_TYPE_ROTATION_VECTOR:
			name = "Rotation vector";
			break;
		case SENSOR_TYPE_OTHER:
			name = "Other";
			break;
		}
		return name;
	}

	public String getFilterTypeName() {
		String name = "UNKNOWN";
		switch (filterType) {
		case TYPE_FILTER_HIGH_PASS:
			name = "High pass";
			break;
		case TYPE_FILTER_KALMAN:
			name = "Kalman";
			break;
		case TYPE_FILTER_LOW_PASS:
			name = "Low pass";
			break;
		case TYPE_FILTER_NONE:
			name = "None";
			break;
		}
		return name;
	}

	public void clear() {
		sensorDatas.clear();
	}

	public void onAccuracyChanged(Sensor sensor, int accuracy) {
		// TODO Auto-generated method stub
	}

	public void onSensorChanged(SensorEvent event) {
//		Log.i("SENSOR DELAY","Delay: "+event.sensor.getMinDelay()+" ms");
		addSensorData(new SensorData(event.timestamp, event.values));
	}

	public static void setSensorDataFilterType(int type, boolean withAdaptative) {
		magneticFieldData.setFilterType(type);
		accelerometerData.setFilterType(type);
		linearAccelData.setFilterType(type);
		gravityData.setFilterType(type);
		orientationData.setFilterType(type);
		gyroscopeData.setFilterType(type);
		
		magneticFieldData.isWithAdaptativeFilter(withAdaptative);
		accelerometerData.isWithAdaptativeFilter(withAdaptative);
		linearAccelData.isWithAdaptativeFilter(withAdaptative);
		gravityData.isWithAdaptativeFilter(withAdaptative);
		orientationData.isWithAdaptativeFilter(withAdaptative);
		gyroscopeData.isWithAdaptativeFilter(withAdaptative);
		rotationData.isWithAdaptativeFilter(withAdaptative);
	}

	public float getCalibrationX() {
		return calibrationX;
	}

	public float getCalibrationY() {
		return calibrationY;
	}

	public float getCalibrationZ() {
		return calibrationZ;
	}

	public boolean isWithCalibration() {
		return withCalibration;
	}

	public void isWithCalibration(boolean is) {
		withCalibration = is;
	}
	
	public void setDelay(int delay) {
		this.sensorDelay = delay;
		unregister();
		register();
	}
    /*
     * Code provided by :
     * http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/	
     */
    private float[] matrixMultiplication(float[] A, float[] B) {
        float[] result = new float[9];
     
        result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
        result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
        result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
     
        result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
        result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
        result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
     
        result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
        result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
        result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
     
        return result;
    }

    private float[] getRotationMatrixFromOrientation(float[] o) {
        float[] xM = new float[9];
        float[] yM = new float[9];
        float[] zM = new float[9];
     
        float sinX = (float)Math.sin(o[1]);
        float cosX = (float)Math.cos(o[1]);
        float sinY = (float)Math.sin(o[2]);
        float cosY = (float)Math.cos(o[2]);
        float sinZ = (float)Math.sin(o[0]);
        float cosZ = (float)Math.cos(o[0]);
     
        // rotation about x-axis (pitch)
        xM[0] = 1.0f; xM[1] = 0.0f; xM[2] = 0.0f;
        xM[3] = 0.0f; xM[4] = cosX; xM[5] = sinX;
        xM[6] = 0.0f; xM[7] = -sinX; xM[8] = cosX;
     
        // rotation about y-axis (roll)
        yM[0] = cosY; yM[1] = 0.0f; yM[2] = sinY;
        yM[3] = 0.0f; yM[4] = 1.0f; yM[5] = 0.0f;
        yM[6] = -sinY; yM[7] = 0.0f; yM[8] = cosY;
     
        // rotation about z-axis (azimuth)
        zM[0] = cosZ; zM[1] = sinZ; zM[2] = 0.0f;
        zM[3] = -sinZ; zM[4] = cosZ; zM[5] = 0.0f;
        zM[6] = 0.0f; zM[7] = 0.0f; zM[8] = 1.0f;
     
        // rotation order is y, x, z (roll, pitch, azimuth)
        float[] resultMatrix = matrixMultiplication(xM, yM);
        resultMatrix = matrixMultiplication(zM, resultMatrix);
        return resultMatrix;
    }
	// This function is borrowed from the Android reference
	// at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
	// It calculates a rotation vector from the gyroscope angular speed values.
    private void getRotationVectorFromGyro(float[] gyroValues,
            float[] deltaRotationVector,
            double timeFactor)
	{
		float[] normValues = new float[3];
		
		// Calculate the angular speed of the sample
		float omegaMagnitude =
		(float)Math.sqrt(gyroValues[0] * gyroValues[0] +
		gyroValues[1] * gyroValues[1] +
		gyroValues[2] * gyroValues[2]);
		
		// Normalize the rotation vector if it's big enough to get the axis
		if(omegaMagnitude > MULT_NANO) {
		normValues[0] = gyroValues[0] / omegaMagnitude;
		normValues[1] = gyroValues[1] / omegaMagnitude;
		normValues[2] = gyroValues[2] / omegaMagnitude;
		}
		
		// Integrate around this axis with the angular speed by the timestep
		// in order to get a delta rotation from this sample over the timestep
		// We will convert this axis-angle representation of the delta rotation
		// into a quaternion before turning it into the rotation matrix.
		float thetaOverTwo = (float) (omegaMagnitude * timeFactor);
		float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
		float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
		deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
		deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
		deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
		deltaRotationVector[3] = cosThetaOverTwo;
	}    

}
